#include "ros/ros.h"
#include "sensor_msgs/NavSatFix.h"
#include "sensor_msgs/Imu.h"
#include "std_msgs/Float64.h"
#include "ekf_class.h"
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;

const double FREQUENCY = 10;

ekf estimator;
ros::Publisher loc_publisher,spd_publisher;
void GPSCallback(const sensor_msgs::NavSatFix::ConstPtr& msg)
{
	estimator.setGPSMeasurement(*msg);
}
void IMUCallback(const sensor_msgs::Imu::ConstPtr& msg)
{
	estimator.setIMUMeasurement(*msg);
}
void GasPedalStateCallback(const std_msgs::Float64& msg)
{
	estimator.setGasPedalState(msg);
}
void publish_location_speed()
{
  loc_publisher.publish(estimator.getEstimatedPose());
  spd_publisher.publish(estimator.getEstimatedSpeed());
}
int main(int argc, char **argv)
{
  cout << "EKF initializing..." << endl;
  ros::init(argc, argv, "ekf");
  ros::NodeHandle n;
  /**
   * subscribe to GPS, IMU and the speed topic.
   */
  ros::Subscriber gps_sub = n.subscribe("/SENSORS/GPS", 1000, GPSCallback);
  ros::Subscriber imu_sub = n.subscribe("/SENSORS/IMU", 1000, IMUCallback);
  //ros::Subscriber pedal_sub = n.subscribe("/drivingVic/gas_pedal/state", 1000, GasPedalStateCallback);
  /**
   * Generate publishers - location and speed
   */
  loc_publisher = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("/LOC/Pose",FREQUENCY);
  spd_publisher = n.advertise<geometry_msgs::TwistStamped>("/LOC/Velocity",FREQUENCY);
  ros::Rate loop_rate(FREQUENCY); // loop rate of 10Hz

  while(ros::ok())
  {
    estimator.estimator();
    publish_location_speed();
    ros::spinOnce();
    loop_rate.sleep();
  }
  
  return 0;
}
